// --------------------------------------------------------------------------------------------------------------------
// <copyright file="DHParameter.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Kinematics
{
    using System;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// A class to represent a DENAVIT-HARTENBERG parameter and to compute the transformation matrix
    /// of this DH parameter.
    /// As for the representation of a DH parameter, we follow the convention of the book
    /// "Introduction to Robotics - Mechanics and Control" by John J. Craig
    /// ---
    /// The four atomic transformations that are involved in a DH parameter are as follows (in order).
    /// They together define the transformation from frame i-1 to frame i.
    /// alpha_i-1: rotation around the X axis of the frame i-1 to establish an intermediate frame t1
    /// a_i-1: translation along the X axis of the intermediate frame t1 to establish an intermediate frame t2
    /// theta_i: rotation around the Z axis of the intermediate frame t2 to establish an intermediate frame t3
    /// d_i: translation along the Z axis of the intermediate frame t3 to reach the final frame i
    /// ---
    /// Joint variable refers to the joint angle when the joint is revolute; When the joint is prismatic, it refers to the linear movement.
    /// ---
    /// The transformation matrix interpreted by this DH parameter can be calculated by
    /// revolute joint: T = Rotation_x(ParameterAlpha) * Translation_x(ParameterA) * Rotation_z(joint_variable) * Translation_z(ParameterD)
    /// prismatic joint: T = Rotation_x(ParameterAlpha) * Translation_x(ParameterA) * Rotation_z(ParameterTheta) * Translation_z(joint_variable)
    /// ---
    /// </summary>
    [DataContract]
    public class DHParameter
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="DHParameter" /> class.
        /// These parameters are corresponding to the ones in the book
        /// "Introduction to Robotics - Mechanics and Control" by John J. Craig
        /// </summary>
        /// <param name="type">type of the joint this DH parameter is associated with</param>
        /// <param name="alpha">parameter alpha, in radians</param>
        /// <param name="a">parameter a, can be in meters or millimeters depending on user's preference</param>
        /// <param name="theta">parameter theta, in radians, NaN should be passed in when constructing a revolute joint since theta here is not used</param>
        /// <param name="d">constant offset for parameter d, can be in meters or millimeters depending on user's selection, NaN should be passed in when constructing a prismatic joint since d here is not used</param>
        private DHParameter(JointType type, double alpha, double a, double theta, double d)
        {
            this.Type = type;
            this.ParameterAlpha = alpha;
            this.ParameterA = a;
            this.ParameterTheta = theta;
            this.ParameterD = d;

            if (JointType.REVOLUTE == type && !double.IsNaN(theta))
            {
                throw new System.ArgumentException("Please make sure set theta to Nan if this joint is revolute");
            }
            else if (JointType.PRISMATIC == type && !double.IsNaN(d))
            {
                throw new System.ArgumentException("Please make sure set d to Nan if this joint is prismatic");
            }
        }

        /// <summary>
        /// Prevents a default instance of the <see cref="DHParameter" /> class from being created.
        /// </summary>
        private DHParameter()
        {
        }

        /// <summary>
        /// Gets the parameter Alpha, rotation along X, in radians
        /// </summary>
        [DataMember]
        public double ParameterAlpha { get; private set; }

        /// <summary>
        /// Gets the parameter A, translation along X, can be in meters or millimeters depending on user's preference
        /// </summary>
        [DataMember]
        public double ParameterA { get; private set; }

        /// <summary>
        /// Gets the parameter Theta, rotation along Z, in radians
        /// This variable is set to NaN and is ignored during computation when the joint is REVOLUTE
        /// </summary>
        [DataMember]
        public double ParameterTheta { get; private set; }

        /// <summary>
        /// Gets the parameter D, translation along Z, can be in meters or millimeters depending on user's preference
        /// This variable is set to NaN and is ignored during computation when the joint is PRISMATIC
        /// </summary>
        [DataMember]
        public double ParameterD { get; private set; }

        /// <summary>
        /// Gets the type of the joint that this DH parameter is associated with
        /// </summary>
        [DataMember]
        public JointType Type { get; private set; }

        /// <summary>
        /// Create DH parameter for a revolute joint, parameter theta is ignored
        /// </summary>
        /// <param name="alpha">parameter alpha, rotation along the X axis in radians</param>
        /// <param name="a">parameter a, translation along the X axis, can be in meters or millimeters depending on user's preference</param>
        /// <param name="d">constant offset for parameter d, translation along the Z axis, can be in meters or millimeters depending on user's selection, NaN should be passed in when constructing a prismatic joint since d here is not used</param>
        /// <returns>A DH parameter</returns>
        public static DHParameter CreateDHParameterForRevoluteJoint(double alpha, double a, double d)
        {
            return new DHParameter(JointType.REVOLUTE, alpha, a, double.NaN, d);
        }

        /// <summary>
        /// Create DH parameter for a prismatic joint, parameter d is ignored
        /// </summary>
        /// <param name="alpha">parameter alpha, rotation along the X axis in radians</param>
        /// <param name="a">parameter a, translation along X axis, can be in meters or millimeters depending on user's preference</param>
        /// <param name="theta">parameter theta, rotation along Z axis in radians, NaN should be passed in when constructing a revolute joint since theta here is not used</param>
        /// <returns>A DH parameter</returns>
        public static DHParameter CreateDHParameterForPrismaticJoint(double alpha, double a, double theta)
        {
            return new DHParameter(JointType.PRISMATIC, alpha, a, theta, double.NaN);
        }

        /// <summary>
        /// Gets the corresponding transformation matrix that is specified by this DH parameter with given joint variable
        /// The input value will be used as component d if the joint type is prismatic
        /// The input value will be used component theta if the joint  type is revolute
        /// The calculation is given in the equation 3.6 of the book
        /// "Introduction to Robotics - Mechanics and Control" by John J. Craig
        /// </summary>
        /// <param name="value">the value of the joint variable, this is used depending on the type of the corresponding joint</param>
        /// <returns>The transformation matrix which specifies the transformation from the previous joint to the current one</returns>
        public Matrix4 GetTransformMatrix(double value)
        {
            Matrix4 transform = new Matrix4();

            // if the joint is revolute, argument value is used as the theta component
            // if the joint is prismatic, argument value is used as the d components
            if (JointType.REVOLUTE == this.Type)
            {
                double cosAlpha = Math.Cos(this.ParameterAlpha);
                double sinAlpha = Math.Sin(this.ParameterAlpha);
                double cosTheta = Math.Cos(value);
                double sinTheta = Math.Sin(value);

                transform[0, 0] = cosTheta;
                transform[0, 1] = -sinTheta;
                transform[0, 2] = 0;

                transform[1, 0] = sinTheta * cosAlpha;
                transform[1, 1] = cosTheta * cosAlpha;
                transform[1, 2] = -sinAlpha;

                transform[2, 0] = sinTheta * sinAlpha;
                transform[2, 1] = cosTheta * sinAlpha;
                transform[2, 2] = cosAlpha;

                transform[0, 3] = this.ParameterA;
                transform[1, 3] = -sinAlpha * this.ParameterD;
                transform[2, 3] = cosAlpha * this.ParameterD;

                transform[3, 0] = transform[3, 1] = transform[3, 2] = 0;
                transform[3, 3] = 1;
            }
            else if (JointType.PRISMATIC == this.Type)
            {
                double cosAlpha = Math.Cos(this.ParameterAlpha);
                double sinAlpha = Math.Sin(this.ParameterAlpha);
                double cosTheta = Math.Cos(this.ParameterTheta);
                double sinTheta = Math.Sin(this.ParameterTheta);

                transform[0, 0] = cosTheta;
                transform[0, 1] = -sinTheta;
                transform[0, 2] = 0;

                transform[1, 0] = sinTheta * cosAlpha;
                transform[1, 1] = cosTheta * cosAlpha;
                transform[1, 2] = -sinAlpha;

                transform[2, 0] = sinTheta * sinAlpha;
                transform[2, 1] = cosTheta * sinAlpha;
                transform[2, 2] = cosAlpha;

                transform[0, 3] = this.ParameterA;
                transform[1, 3] = -sinAlpha * value;
                transform[2, 3] = cosAlpha * value;

                transform[3, 0] = transform[3, 1] = transform[3, 2] = 0;
                transform[3, 3] = 1;
            }
            else
            {
                throw new System.InvalidOperationException("unidentified joint type used");
            }

            return transform;
        }
    }
}
